#pragma config(Sensor, in1,    TubeSensor,          sensorLineFollower)
#pragma config(Sensor, dgtl1,  PotArm,              sensorQuadEncoder)
#pragma config(Motor,  port1,           ArmLL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           DriveLF,       tmotorNormal, openLoop)
#pragma config(Motor,  port3,           DriveLB,       tmotorNormal, openLoop)
#pragma config(Motor,  port4,           DriveRF,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           DriveRB,       tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port6,           ArmLU,         tmotorNormal, openLoop)
#pragma config(Motor,  port7,           ArmRU,         tmotorNormal, openLoop)
#pragma config(Motor,  port8,           collector1,    tmotorNormal, openLoop)
#pragma config(Motor,  port9,           collector2,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          ArmRL,         tmotorNormal, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "alpha_lib.c" //Main Funtion Library
//--/ Drive /-------------------------------------------------------/
int DriveMode;
bool filter;

//--/ PID /-------------------------------------------------------/
PIDController arm;
int startpoint = 0;
int goal_value = startpoint;
int change = -1000;
int k_P = 4;
int k_I = 0;
int k_D = 0;

//--/ Arm Position /----------------------------------------------/
int low_stop = 2025;
int drive = 1400;
int low_goal = 780;
int high_goal = 415;
int high_stop = 375;
int preset = 0;
int armMoved = 0;

void pre_auton() {
  //--/ Drive /-------------------------------------------------------/
  DriveMode=0;
  //--/ Arm Points /-----------------------/
  goal_value = SensorValue[PotArm];
  //--/ PID /------------------------------/
  init(arm);

  setPIDs(arm, k_P, k_I, k_D);
  setSetpoint(arm, goal_value);

  enable(arm);

  arm.k_P = 4;
}

task autonomous() {
  lock_msec(127, 20);
  drive_msec(-127, 2000); //Drive backwards ar goal or other
}

task usercontrol() {
    pre_auton();

    while (true) {
/*******DRIVE******************************************************************/
        /* Original drive code, basic, no input scaling
          motor[DriveLB] = motor[DriveLF] = checkDeadZone(vexRT[Ch3]);
          motor[DriveRB] = motor[DriveRF] = checkDeadZone(vexRT[Ch2]);*/

        /*//Allows buttons to change drive mode (scaling function)
        if(vexRT[Btn8U] == 1){// no scaling
                DriveMode = 0;
                filter=false;
            }
        if(vexRT[Btn8R] == 1){//Squaring function, concave up
                DriveMode = 1;
                filter=false;
            }
        if(vexRT[Btn8D] == 1){//arcsin function, concave up
                DriveMode = 2;
                filter=false;
            }
        if(vexRT[Btn8L] == 1){//Sets to arctan(x^2) function, concave down
                filter=true;
            }*/
//parameters (left input,right input,scaling (t/f),rate filter,(t/f),DriveMode)
//rate filter disabled by default as constant has not been determined*/
        driveTank(vexRT[Ch3], vexRT[Ch2], true, filter,DriveMode);


/*******ARM********************************************************************/
    short pot = SensorValue[PotArm];
    if (pot > 258) { //if pot is plugged in
    if (vexRT[Btn5D]) { //if down button check we aren't at down stop and move the arm down, also set armMoved counter
      if (pot < low_stop) {
        setArmSpeed(FULL);
        armMoved = 500;
      } else {
        setArmSpeed(0); //go to zero if at our lower limit
      }
      preset = 0;  //disable the preset once the button is pushed
    } else if (vexRT[Btn5U]) { //if up button is pushed
      if (pot > high_stop) { //if we are not at are high limit
        setArmSpeed(-FULL); //let arm go up
        armMoved = 500;     //set armMoved Counter used for pid positioning
      } else {
        setArmSpeed(0);    //if at the limit don't let the arm move
      }
      preset = 0;         //turn off preset positions
    } else {              // if neither button is pushed
      if (preset == 0 && armMoved > 0) { //let the arm steady itself then set the arm setpoint
        setSetpoint(arm, pot);
        armMoved--;
      }
        setArmSpeed(calculatePID(arm,pot)); //use the pid control to control the arm if neither button is pushed
    }

    if(vexRT[Btn7U]) { //arm state machine moves to the next heights height
      preset = 1;       //enables presets
      if (pot > drive + 100) {
        setSetpoint(arm, drive);
      } else if (pot > low_goal + 100) {
        setSetpoint(arm, low_goal);
      } else if (pot > high_goal + 100) {
        setSetpoint(arm, high_goal);
      }
    }
  } else { //if pot gets unplugged revert to old code and if it gets plugged back in hold current position
    preset = 0;
    armMoved = 500;
    setArmSpeed((vexRT[Btn5D] - vexRT[Btn5U]) * FULL);
  }



/*******COLLECTOR**************************************************************/
        motor[collector1] = motor[collector2] = spinners();
    }
}
